import cv2
import numpy as np

class LaserVisionProcessor:
    def __init__(self, width=640, height=480, fps=60):

        self.width = width
        self.height = height
        self.fps = fps
        self.last_offset_x = 0  # 添加偏移量记忆属性
        self.last_offset_y = 0


        self.resized_list = []   # 用来保存所有检测到的 resized 坐标
        # 用于存储上一次成功检测和细分的轮廓点
        self.last_subdivided_contour_points = None

    def morph_treat(self, bin_img):
        blurred = cv2.GaussianBlur(bin_img, (3, 3), 3)
        element = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
        dilated = cv2.dilate(blurred, element, iterations=2)
        eroded = cv2.erode(dilated, element, iterations=2)
        gray = cv2.cvtColor(eroded, cv2.COLOR_BGR2GRAY)
        _, binary = cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY)
        return binary

    def contours_handle(self, pts_list, scalar):
        """
        你的原始 contours_handle 函数。
        输入:
            pts_list (list): 轮廓点列表，每个元素是 [x, y] 或 (x, y)。
            scalar (float): 用于计算偏移量的标量。
        返回:
            np.array: 处理后的轮廓点数组，形状为 (N, 2),N是点的数量。
        """
        count = len(pts_list)
        if count < 2: # 如果点少于2个，无法形成有效的轮廓进行处理
            return np.array(pts_list, dtype=np.int32) # 直接返回

        # 将输入列表转换为Numpy数组，确保是浮点数以便进行向量运算
        pts = np.array(pts_list, dtype=np.float32)

        # dp: 存储轮廓每条边的向量 (p[i+1] - p[i])
        dp = [pts[(i + 1) % count] - pts[i] for i in range(count)]
        
        # ndp: 存储归一化后的边向量（单位向量）
        # 如果向量长度为0（例如重点），则保持原样以避免除零错误
        ndp = []
        for d_vec in dp:
            norm = np.linalg.norm(d_vec)
            if norm != 0:
                ndp.append(d_vec / norm)
            else:
                ndp.append(d_vec) # 保持零向量

        out_pts = [] # 存储新的轮廓点
        for i in range(count):
            prev_idx = (i - 1 + count) % count # 上一个点的索引，确保环绕
            
            # sin_theta: 计算相邻两条归一化边向量的叉积的Z分量 (ndp[prev] 和 ndp[i])
            # 叉积的大小与它们之间夹角的sin值成正比。
            # 对于2D向量 (x1, y1) 和 (x2, y2)，叉积的Z分量是 x1*y2 - y1*x2
            sin_theta = np.cross(ndp[prev_idx], ndp[i])

            # 如果sin_theta接近0，说明两条边几乎共线，跳过此点以避免除以一个很小的值
            if abs(sin_theta) < 1e-6: # 1e-6 是一个小的容差值
                # 可以选择添加原始点，或者像原始代码一样跳过
                # out_pts.append(tuple(pts[i].astype(int)))
                continue # 原始逻辑是跳过

            # orient: 计算从上一条边指向当前边的方向变化 (ndp[i] - ndp[prev])
            # 这个向量的方向大致是该顶点的角平分线的法线方向（向外或向内）
            orient = ndp[i] - ndp[prev_idx]
            
            # shift: 计算偏移向量
            # scalar / sin_theta 是偏移的幅度，乘以orient向量得到实际的偏移
            # sin_theta 越小（角度越尖锐），偏移越大
            shift_vector = orient * (scalar / sin_theta)
            
            new_pt = pts[i] + shift_vector
            out_pts.append(tuple(new_pt.astype(int))) # 将计算出的新点（取整）添加到列表

        return np.array(out_pts, dtype=np.int32) # 返回Numpy数组，形状 (N, 2)

    def subdivide_contour_points(self, contour_points_np, spacing=10):
        """
        对轮廓进行细分，在边上插入点。

        参数:
            contour_points_np (np.array): NumPy 数组，形状为 (N, 2)，代表轮廓顶点 [x, y]。
            spacing (int): 轮廓上点之间的期望近似距离。

        返回:
            np.array: 细分后的新轮廓点，形状为 (M, 1, 2)，M >= N，
                      可以直接用于 cv2.drawContours。
        """
        # 确保输入是 (N,2) 的Numpy数组
        if not isinstance(contour_points_np, np.ndarray) or contour_points_np.ndim != 2 or contour_points_np.shape[1] != 2:
            # 如果输入是 (N,1,2)，则squeeze一下
            if isinstance(contour_points_np, np.ndarray) and contour_points_np.ndim == 3 and contour_points_np.shape[1] == 1 and contour_points_np.shape[2] == 2:
                contour_points_np = contour_points_np.squeeze(axis=1)
            else: # 否则无法处理
                print("错误: subdivide_contour_points 需要 (N,2) 或 (N,1,2) 的Numpy数组输入")
                return contour_points_np.reshape(-1,1,2) if contour_points_np is not None else np.array([])


        if contour_points_np is None or len(contour_points_np) < 2:
            # 如果点少于2个，无法形成边，直接返回原始点（调整为drawContours格式）
            return contour_points_np.reshape(-1, 1, 2) if contour_points_np is not None else np.array([])

        subdivided_pts_list = [] # 用于存储所有点（原始点+新插入的点）
        num_pts = len(contour_points_np)

        for i in range(num_pts):
            p1 = contour_points_np[i]
            p2 = contour_points_np[(i + 1) % num_pts] # 下一个点，对于闭合轮廓会自动回绕到第一个点

            # 1. 将当前边的起始点 p1 加入列表
            subdivided_pts_list.append([p1[0], p1[1]])

            # 2. 计算当前边的向量和长度
            dx = float(p2[0] - p1[0])
            dy = float(p2[1] - p1[1])
            segment_length = np.sqrt(dx*dx + dy*dy)

            # 3. 如果边长大于细分间距，则在这条边上插入点
            if segment_length > spacing:
                # 计算需要在这条边上插入多少个新点
                # 例如：长度25，间距10，则 num_new_points_on_segment = 25/10 = 2 (取整)
                # 我们将在距离p1点 1*spacing, 2*spacing, ... 的位置插入点
                num_intermediate_points = int(segment_length / spacing)

                for j in range(1, num_intermediate_points + 1): # j 从 1 开始，因为p1已经添加了
                    # 当前要插入的点距离 p1 的实际长度
                    current_dist_along_segment = j * spacing
                    
                    # 确保插入的点不会超过 p2 （或非常接近p2）
                    # （如果 segment_length 恰好是 spacing 的倍数，最后一个插入点会与 p2 重合，
                    #  但 p2 会在下一次迭代中作为 p1 被添加，所以这里严格小于）
                    if current_dist_along_segment < segment_length:
                        ratio = current_dist_along_segment / segment_length
                        new_x = p1[0] + ratio * dx
                        new_y = p1[1] + ratio * dy
                        subdivided_pts_list.append([int(round(new_x)), int(round(new_y))])
                    else:
                        # 如果计算出的点超出了当前段，则停止在该段上添加点
                        break 
            # p2 将在下一次循环中作为 p1 被添加

        if not subdivided_pts_list:
             return np.array([]) # 如果列表为空，返回空数组

        # 将点列表转换为 cv2.drawContours 所需的 NumPy 数组格式 (M, 1, 2)
        return np.array(subdivided_pts_list, dtype=np.int32).reshape(-1, 1, 2)


  
    def get_contours(self, img_dil, frame, subdivision_spacing=5):
        contours, _ = cv2.findContours(img_dil, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)

        output_frame = frame.copy()
        found_new_contour_this_frame = False  # 标记本帧是否找到了新轮廓

        roi_x_offset = 130  # ROI 起始横坐标

        for cnt in contours:
            area = cv2.contourArea(cnt)
            #print(f"Area: {area}")
    
            cnt[:, :, 0] += roi_x_offset
            # cv2.drawContours(output_frame, [cnt], -1, (0, 0, 255), 2)

            if 14000 < area < 20000:  # 面积阈值
                peri = cv2.arcLength(cnt, True)
                approx = cv2.approxPolyDP(cnt, 0.02 * peri, True)

                if 4 <= len(approx) <= 5:
                    x, y, w, h = cv2.boundingRect(approx)
                    asp = w / float(h) if h != 0 else 0
                    current_obj_type = "Square" if 0.95 < asp < 1.05 else "Rectangle"

                    hull = cv2.convexHull(cnt)
                    resized_original_shape = self.contours_handle(hull.squeeze().tolist(), -4.15)  # scalar 值

                    if resized_original_shape.size > 0 and len(resized_original_shape) >= 2:
                        current_subdivided_contour = self.subdivide_contour_points(resized_original_shape, subdivision_spacing)

                        if current_subdivided_contour.size > 0:
                            # 将 ROI 区域的轮廓点映射回原图坐标系
                            mapped_contour = current_subdivided_contour.copy()
                            # mapped_contour[:, 0, 0] += roi_x_offset

                            # 成功检测并细分了新轮廓
                            found_new_contour_this_frame = True

                            # 更新保存的轮廓点和信息
                            self.last_subdivided_contour_points = mapped_contour

                            # 绘制当前检测到的轮廓 (例如用绿色)
                            cv2.drawContours(output_frame, [mapped_contour], -1, (0, 255, 0), 2)  # 绿色

                            for point_group in mapped_contour:
                                point = tuple(point_group[0])

                            return output_frame, mapped_contour

        # 如果循环结束，意味着在当前帧没有找到新的符合条件的轮廓
        if not found_new_contour_this_frame:
            if self.last_subdivided_contour_points is not None:
                for point_group in self.last_subdivided_contour_points:
                    point = tuple(point_group[0])
                # 返回上一次的轮廓点
                return output_frame, self.last_subdivided_contour_points
            else:
                return output_frame, None

        return output_frame, None


    def get_circle(self, img_dil, frame):

        cx, cy = None, None
        contours, _ = cv2.findContours(img_dil, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)

        for cnt in contours:
            
            area = cv2.contourArea(cnt)
            #print(area)
            
            if 10 < area < 500:
                M = cv2.moments(cnt)

                if M['m00'] != 0:
                    cx = int(M['m10'] / M['m00']) + 130
                    cy = int(M['m01'] / M['m00'])
                    # cv2.circle(frame,(cx, cy),5,(255, 0, 0),-1)
                    #print("Circle_detect:", cx, cy)

            else:
                print("no")

        return frame, cx, cy

                    
    
# if  __name__ == "__main__":
#     detector = LaserVisionProcessor()
#     detector.detect_lines("")